clear;close all;clc;
dbstop if error;
data = '20251009';
numImages=20;%%%%总组图片数
mkdir("SysCalibrtionResults\"+data);%%%%创建文件夹
Pro_width = 1280;
Pro_height = 720;
N = 20;
T = 20;
f = Pro_width / T;
n=log2(f);%互补张数
detector=vision.calibration.monocular.SymmetricCircleGridDetector();
%标定板有9行11列 以用来确定x 轴和y轴 9为为y方向 11为x方向
PatternDims=[9,11];
N_point=PatternDims(1)*PatternDims(2);
centerDistance=10;
WorldPoints=generateWorldPoints(detector,'PatternDims',PatternDims,'CenterDistance',centerDistance);
%%
%识别相机特征点
Saved_Picture_location ="SysCalibrtionResults\"+data+'\Result_CalibrationPicture\';
File_Picture="..\解相位\Pic\"+data+"\calibration\";
mkdir(Saved_Picture_location);
imageFileNames=[];
for i=1:numImages
     I = imread(char(File_Picture+num2str(i)+'\'+num2str(2*(N+n+1)+1)+'.bmp'));%读白光图片
     disp(File_Picture+num2str(i)+'\'+num2str(2*(N+n+1)+1)+'.bmp');
     imwrite(I,Saved_Picture_location+num2str(i)+'.bmp');
     imageFileNames{end+1}= char(Saved_Picture_location+num2str(i)+'.bmp');
     disp(imageFileNames{i});
end
%ImageUesd为 ImageNumber x1  逻辑数组
[Cam_ImagePoints,ImageUsed]=detectPatternPoints(detector,imageFileNames,'PatternDims',PatternDims,'CircleColor','white');
%判断成功张数
num=0;
for i=1:numImages
    if(ImageUsed(i,1)==1)
        num=num+1;
    end
end


if(numImages==num)
    disp('提取角点成功');
        for i=1:numImages
                    if(ImageUsed(i,1)==1)
                       I =imread(Saved_Picture_location+num2str(i)+'.bmp');
                       J = insertText(I,Cam_ImagePoints(:,:,i),1:size(Cam_ImagePoints(:,:,i)));
                       J = insertMarker(J,Cam_ImagePoints(:,:,i),'x',Color="green",Size=30);
                       imwrite(J,Saved_Picture_location+num2str(i)+'_Marker.bmp');    
                    end
        end 
else
    error('提取角点失败');
end

%%
%读取投影仪的相位 插值计算投影仪特征点
% P=width/f(1,3)/2/pi;
% Q=height/f(1,3)/2/pi;
P=T/2/pi;
Q=T/2/pi;
mkdir("SysCalibrtionResults\"+data+'\Result_ProjectorPoints');
Pro_ImagePoints=double(zeros(N_point,2,numImages));
w=2;
for n = 1:numImages
     H = load("..\解相位\phase\"+data+"\hPhases_"+num2str(n)+".mat");
     H_Absulate(:,:,n) =H.PHDeformed_Unwrap;
     V= load("..\解相位\phase\"+data+"\vPhases_"+num2str(n)+".mat");
     V_Absulate(:,:,n) = V.PVDeformed_Unwrap;
      for j =1:N_point
       %角点横纵坐标
       x=Cam_ImagePoints(j,1,n);
       y=Cam_ImagePoints(j,2,n);
       x_round = round(Cam_ImagePoints(j,1,n));%角点横坐标
       y_round = round(Cam_ImagePoints(j,2,n));%角点纵坐标
       xs =zeros(1,2*w+1);
       ys =zeros(1,2*w+1);
       phas_x=zeros(1,2*w+1);
       phas_y=zeros(1,2*w+1);

       ii=1;
       for jj =-1*w:w
            xs(1,ii)=x_round+jj;
            ys(1,ii)=y_round+jj;
            phas_x(1,ii)=V_Absulate(y_round,xs(1,ii),n);
            phas_y(1,ii)=H_Absulate(ys(1,ii),x_round,n);
            ii=ii+1;
       end
        pha_x=spline(xs,phas_x,x);
        pha_y=spline(ys,phas_y,y);
       %获得亚像素点的相位值
        Pro_ImagePoints(j,:,n)=[pha_x *P,pha_y*Q];
      end
    %显示投影仪点的映射
    h=figure;
    scatter(Pro_ImagePoints(:,1,n),Pro_ImagePoints(:,2,n));
    exportgraphics(h,"SysCalibrtionResults\"+data+'\Result_ProjectorPoints\'+num2str(n)+'.png','Resolution',600);
    close(h);
end


%直接调包
cam_proj_points = zeros(99,2,numImages,2);
cam_proj_points(:,:,:,1) = Cam_ImagePoints; 
cam_proj_points(:,:,:,2) = Pro_ImagePoints;

CamParams = estimateCameraParameters(Cam_ImagePoints,WorldPoints, 'EstimateTangentialDistortion',true);
figure;
showExtrinsics(CamParams);
figure;
showReprojectionErrors(CamParams);


ProParams = estimateCameraParameters(Pro_ImagePoints,WorldPoints, 'EstimateTangentialDistortion',true);
figure;
showExtrinsics(ProParams);
figure;
showReprojectionErrors(ProParams);


stereoParams = estimateCameraParameters(cam_proj_points, WorldPoints,'EstimateTangentialDistortion',true);



h=figure;showExtrinsics(stereoParams);title('System Calibration Result');
mkdir("SysCalibrtionResults\"+data+'\Sys_Pic\');
mkdir("SysCalibrtionResults\"+data+"\Sys_Result\");
exportgraphics(h,"SysCalibrtionResults\"+data+'\Sys_Pic\'+'showExtrinsics.png','Resolution',600);
h=figure;showReprojectionErrors(stereoParams);title('System Calibration Result');
exportgraphics(h,"SysCalibrtionResults\"+data+'\Sys_Pic\'+'System_Calibration_Result.png','Resolution',600);
save("SysCalibrtionResults\"+data+"\Sys_Result"+'\stereoParams.mat', 'stereoParams');



cameraParams_IntrinsicMatrix=stereoParams.CameraParameters1.K;
Projector_IntrinsicMatrix=stereoParams.CameraParameters2.K;

Pro2Camera_R=stereoParams.PoseCamera2.R;
Pro2Camera_T=stereoParams.PoseCamera2.Translation';

Camera_radialDistortion=stereoParams.CameraParameters1.RadialDistortion;
Camera_tangentialDistortion=stereoParams.CameraParameters1.TangentialDistortion;
Pro_radialDistortion=stereoParams.CameraParameters2.RadialDistortion;
Pro_tangentialDistortion=stereoParams.CameraParameters2.TangentialDistortion;
FundamentalMatrix=stereoParams.FundamentalMatrix;


Hc = cameraParams_IntrinsicMatrix * [[1,0,0;0,1,0;0,0,1], [0;0;0]];
Hp = Projector_IntrinsicMatrix * [Pro2Camera_R,Pro2Camera_T];

% varClass: the variable class waiting for write:
%           'i': for int
%           'f': for float
% flag    : Write mode
%           'w': for write
%           'a': for append

% dym_matlab2opencv(Hc,"SysCalibrtionResults\"+data+'\Sys_Result\Hc.yml','w','f');
% dym_matlab2opencv(Hp,"SysCalibrtionResults\"+data+'\Sys_Result\Hp.yml','w','f');
% dym_matlab2opencv(Camera_radialDistortion,"SysCalibrtionResults\"+data+'\Sys_Result\Camera_radialDistortion.yml','w','f');
% dym_matlab2opencv(Pro_radialDistortion,"SysCalibrtionResults\"+data+'\Sys_Result\Pro_radialDistortion.yml','w','f');
% dym_matlab2opencv(Camera_tangentialDistortion,"SysCalibrtionResults\"+data+'\Sys_Result\Camera_tangentialDistortion.yml','w','f');
% dym_matlab2opencv(Pro_tangentialDistortion,"SysCalibrtionResults\"+data+'\Sys_Result\Pro_tangentialDistortion.yml','w','f');
% dym_matlab2opencv(Pro_tangentialDistortion,"SysCalibrtionResults\"+data+'\Sys_Result\FundamentalMatrix.yml','w','f');
% 
% 



dym_matlab2opencv(Hc,"SysCalibrtionResults\"+data+'\Sys_Result\Results.yml','w','f');
dym_matlab2opencv(Hp,"SysCalibrtionResults\"+data+'\Sys_Result\Results.yml','a','f');
dym_matlab2opencv(cameraParams_IntrinsicMatrix,"SysCalibrtionResults\"+data+'\Sys_Result\Results.yml','a','f');
dym_matlab2opencv(Projector_IntrinsicMatrix,"SysCalibrtionResults\"+data+'\Sys_Result\Results.yml','a','f');
dym_matlab2opencv(Camera_radialDistortion,"SysCalibrtionResults\"+data+'\Sys_Result\Results.yml','a','f');
dym_matlab2opencv(Pro_radialDistortion,"SysCalibrtionResults\"+data+'\Sys_Result\Results.yml','a','f');
dym_matlab2opencv(Camera_tangentialDistortion,"SysCalibrtionResults\"+data+'\Sys_Result\Results.yml','a','f');
dym_matlab2opencv(Pro_tangentialDistortion,"SysCalibrtionResults\"+data+'\Sys_Result\Results.yml','a','f');
dym_matlab2opencv(FundamentalMatrix,"SysCalibrtionResults\"+data+'\Sys_Result\Results.yml','a','f');


save("SysCalibrtionResults\"+data+"\Sys_Result"+'\Hc.mat', 'Hc');
save("SysCalibrtionResults\"+data+"\Sys_Result"+'\Hp.mat', 'Hp');



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 3D recosntruction with the calibrated triangular stereo model.
% Related Reference:
% "Three-dimensional shape measurement: Avoiding matrix inversion"
% Chenbo Zhang, Mingde Zhang, Chen Chen
% last modified on 03/07/2023
% by Chenbo Zhang (Email: zcb@2019.cqut.edu.cn)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% set M1 and M2
Mc = Hc;
Mp = Hp;
for i = 1:3
    for j = 1:4
        eval(['mc',num2str(i),num2str(j),'=','Mc(',num2str(i),',',num2str(j),');']);
        eval(['mp',num2str(i),num2str(j),'=','Mp(',num2str(i),',',num2str(j),');']);
    end
end

X0  =   mc12*mc23*mp14 - mc12*mc24*mp13 - mc13*mc22*mp14 + mc13*mc24*mp12 + mc14*mc22*mp13 - mc14*mc23*mp12;
Xxc =   mc22*mc33*mp14 - mc22*mc34*mp13 - mc23*mc32*mp14 + mc23*mc34*mp12 + mc24*mc32*mp13 - mc24*mc33*mp12;
Xxp =   mc12*mc24*mp33 - mc12*mc23*mp34 + mc13*mc22*mp34 - mc13*mc24*mp32 - mc14*mc22*mp33 + mc14*mc23*mp32;
Xyc =   mc12*mc34*mp13 - mc12*mc33*mp14 + mc13*mc32*mp14 - mc13*mc34*mp12 - mc14*mc32*mp13 + mc14*mc33*mp12;
Xxcxp = mc22*mc34*mp33 - mc22*mc33*mp34 + mc23*mc32*mp34 - mc23*mc34*mp32 - mc24*mc32*mp33 + mc24*mc33*mp32;
Xxpyc = mc12*mc33*mp34 - mc12*mc34*mp33 - mc13*mc32*mp34 + mc13*mc34*mp32 + mc14*mc32*mp33 - mc14*mc33*mp32;

Y0 =    mc11*mc23*mp14 - mc11*mc24*mp13 - mc13*mc21*mp14 + mc13*mc24*mp11 + mc14*mc21*mp13 - mc14*mc23*mp11;
Yxc =   mc21*mc33*mp14 - mc21*mc34*mp13 - mc23*mc31*mp14 + mc23*mc34*mp11 + mc24*mc31*mp13 - mc24*mc33*mp11;
Yxp =   mc11*mc24*mp33 - mc11*mc23*mp34 + mc13*mc21*mp34 - mc13*mc24*mp31 - mc14*mc21*mp33 + mc14*mc23*mp31;
Yyc =   mc11*mc34*mp13 - mc11*mc33*mp14 + mc13*mc31*mp14 - mc13*mc34*mp11 - mc14*mc31*mp13 + mc14*mc33*mp11;
Yxcxp = mc21*mc34*mp33 - mc21*mc33*mp34 + mc23*mc31*mp34 - mc23*mc34*mp31 - mc24*mc31*mp33 + mc24*mc33*mp31;
Yxpyc = mc11*mc33*mp34 - mc11*mc34*mp33 - mc13*mc31*mp34 + mc13*mc34*mp31 + mc14*mc31*mp33 - mc14*mc33*mp31;

Z0 =    mc11*mc22*mp14 - mc11*mc24*mp12 - mc12*mc21*mp14 + mc12*mc24*mp11 + mc14*mc21*mp12 - mc14*mc22*mp11;
Zxc =   mc21*mc32*mp14 - mc21*mc34*mp12 - mc22*mc31*mp14 + mc22*mc34*mp11 + mc24*mc31*mp12 - mc24*mc32*mp11;
Zxp =   mc11*mc24*mp32 - mc11*mc22*mp34 + mc12*mc21*mp34 - mc12*mc24*mp31 - mc14*mc21*mp32 + mc14*mc22*mp31;
Zyc =   mc11*mc34*mp12 - mc11*mc32*mp14 + mc12*mc31*mp14 - mc12*mc34*mp11 - mc14*mc31*mp12 + mc14*mc32*mp11;
Zxcxp = mc21*mc34*mp32 - mc21*mc32*mp34 + mc22*mc31*mp34 - mc22*mc34*mp31 - mc24*mc31*mp32 + mc24*mc32*mp31;
Zxpyc = mc11*mc32*mp34 - mc11*mc34*mp32 - mc12*mc31*mp34 + mc12*mc34*mp31 + mc14*mc31*mp32 - mc14*mc32*mp31;

S0 =    mc11*mc22*mp13 - mc11*mc23*mp12 - mc12*mc21*mp13 + mc12*mc23*mp11 + mc13*mc21*mp12 - mc13*mc22*mp11;
Sxc =   mc21*mc32*mp13 - mc21*mc33*mp12 - mc22*mc31*mp13 + mc22*mc33*mp11 + mc23*mc31*mp12 - mc23*mc32*mp11;
Sxp =   mc11*mc23*mp32 - mc11*mc22*mp33 + mc12*mc21*mp33 - mc12*mc23*mp31 - mc13*mc21*mp32 + mc13*mc22*mp31;
Syc =   mc11*mc33*mp12 - mc11*mc32*mp13 + mc12*mc31*mp13 - mc12*mc33*mp11 - mc13*mc31*mp12 + mc13*mc32*mp11;
Sxcxp = mc21*mc33*mp32 - mc21*mc32*mp33 + mc22*mc31*mp33 - mc22*mc33*mp31 - mc23*mc31*mp32 + mc23*mc32*mp31;
Sxpyc = mc11*mc32*mp33 - mc11*mc33*mp32 - mc12*mc31*mp33 + mc12*mc33*mp31 + mc13*mc31*mp32 - mc13*mc32*mp31;

% set M1
M1 = [X0 Xxc Xxp Xyc Xxcxp Xxpyc;
      Y0 Yxc Yxp Yyc Yxcxp Yxpyc;
      Z0 Zxc Zxp Zyc Zxcxp Zxpyc];
            
% set M2
M2 = [S0 Sxc Sxp Syc Sxcxp Sxpyc];

save("SysCalibrtionResults\"+data+"\Sys_Result"+'\M1.mat', 'M1');
save("SysCalibrtionResults\"+data+"\Sys_Result"+'\M2.mat', 'M2');

dym_matlab2opencv(M1,"SysCalibrtionResults\"+data+'\Sys_Result\Results.yml','a','f');
dym_matlab2opencv(M2,"SysCalibrtionResults\"+data+'\Sys_Result\Results.yml','a','f');